#include "bsp_encoder.h"
#include "bsp_motor.h"

static void *MOTOR_Task(void)
{
  printf("motor start  \n");
  encoder_init();
  motor_init();

  int Encoder_All_Now[MAX_MOTOR] = {0};
  Motor_Set_Pwm(0, 2); // 2500
  Motor_Set_Pwm(1, 2);
  Motor_Set_Pwm(2, 2);
  Motor_Set_Pwm(3, 2);
  while (1)
  {
    osDelay(1000);
    Encoder_Update_Count();
    Encoder_Get_ALL(Encoder_All_Now);
    // M1 M2 M3 M4
    printf("1:%d,2:%d,3:%d,4:%d  \n", Encoder_All_Now[0], Encoder_All_Now[1], Encoder_All_Now[2], Encoder_All_Now[3]);
  }
}

static void ExampleEntry(void)
{

  osThreadAttr_t attr;

  attr.name = "MOTOR_Task";
  attr.attr_bits = 0U;
  attr.cb_mem = NULL;
  attr.cb_size = 0U;
  attr.stack_mem = NULL;
  attr.stack_size = 2048;
  attr.priority = osPriorityNormal;

  if (osThreadNew((osThreadFunc_t)MOTOR_Task, NULL, &attr) == NULL)
  {
    printf("[ExampleEntry] create MOTOR_Task NG\n");
  }
  else
  {
    printf("[ExampleEntry] create MOTOR_Task OK\n");
  }
}
SYS_RUN(ExampleEntry);
